/** \file
 * \brief Example code for Simple Open EtherCAT master
 *
 * Usage : simple_test [ifname1]
 * ifname is NIC interface, f.e. eth0
 *
 * This is a minimal test.
 *
 * (c)Arthur Ketels 2010 - 2011
 */

#include <stdio.h>
#include <string.h>
#include <inttypes.h>

#include "ethercat.h"

#define EC_TIMEOUTMON 500

char IOmap[4096] = {0};
OSAL_THREAD_HANDLE thread1;
int expectedWKC;
boolean needlf;
volatile int wkc;
boolean inOP;
uint8 currentgroup = 0;

void simpletest(char *ifname)
{
    int i, j, oloop, iloop, chk;
    needlf = FALSE;
    inOP = FALSE;

   printf("Starting simple test\n");

   /* initialise SOEM, bind socket to ifname */
   if (ec_init(ifname))
   {
      printf("ec_init on %s succeeded.\n",ifname);
      /* find and auto-config slaves */


       if ( ec_config_init(FALSE) > 0 )
      {
         printf("%d slaves found and configured.\n",ec_slavecount);

         ec_config_map(&IOmap);

         ec_configdc();

         printf("Slaves mapped, state to SAFE_OP.\n");
         /* wait for all slaves to reach SAFE_OP state */
         ec_statecheck(0, EC_STATE_SAFE_OP,  EC_TIMEOUTSTATE * 4);

         oloop = ec_slave[0].Obytes;
         if ((oloop == 0) && (ec_slave[0].Obits > 0)) oloop = 1;
         if (oloop > 8) oloop = 8;
         iloop = ec_slave[0].Ibytes;
         if ((iloop == 0) && (ec_slave[0].Ibits > 0)) iloop = 1;
         if (iloop > 8) iloop = 8;

         printf("segments : %d : %d %d %d %d\n",ec_group[0].nsegments ,ec_group[0].IOsegment[0],ec_group[0].IOsegment[1],ec_group[0].IOsegment[2],ec_group[0].IOsegment[3]);

         printf("Request operational state for all slaves\n");
         expectedWKC = (ec_group[0].outputsWKC * 2) + ec_group[0].inputsWKC;
         printf("Calculated workcounter %d\n", expectedWKC);
         ec_slave[0].state = EC_STATE_OPERATIONAL;
         /* send one valid process data to make outputs in slaves happy*/
         ec_send_processdata();
         ec_receive_processdata(EC_TIMEOUTRET);
         /* request OP state for all slaves */
         ec_writestate(0);
         chk = 200;
         /* wait for all slaves to reach OP state */
         do
         {
            ec_send_processdata();
            ec_receive_processdata(EC_TIMEOUTRET);
            ec_statecheck(0, EC_STATE_OPERATIONAL, 50000);
         }
         while (chk-- && (ec_slave[0].state != EC_STATE_OPERATIONAL));
         if (ec_slave[0].state == EC_STATE_OPERATIONAL )
         {
            printf("Operational state reached for all slaves.\n");
            inOP = TRUE;
                /* cyclic loop */
            for(i = 1; i <= 10000; i++)
            {
               ec_send_processdata();
               wkc = ec_receive_processdata(EC_TIMEOUTRET);

                    if(wkc >= expectedWKC)
                    {
                        printf("Processdata cycle %4d, WKC %d , O:", i, wkc);

                        for(j = 0 ; j < oloop; j++)
                        {
                            printf(" %2.2x", *(ec_slave[0].outputs + j));
                        }

                        printf(" I:");
                        for(j = 0 ; j < iloop; j++)
                        {
                            printf(" %2.2x", *(ec_slave[0].inputs + j));
                        }
                        printf(" T:%"PRId64"\r",ec_DCtime);
                        needlf = TRUE;
                    }
                    osal_usleep(5000);

                }
                inOP = FALSE;
            }
            else
            {
                printf("Not all slaves reached operational state.\n");
                ec_readstate();
                for(i = 1; i<=ec_slavecount ; i++)
                {
                    if(ec_slave[i].state != EC_STATE_OPERATIONAL)
                    {
                        printf("Slave %d State=0x%2.2x StatusCode=0x%4.4x : %s\n",
                            i, ec_slave[i].state, ec_slave[i].ALstatuscode, ec_ALstatuscode2string(ec_slave[i].ALstatuscode));
                    }
                }
            }
            printf("\nRequest init state for all slaves\n");
            ec_slave[0].state = EC_STATE_INIT;
            /* request INIT state for all slaves */
            ec_writestate(0);
        }
        else
        {
            printf("No slaves found!\n");
        }
        printf("End simple test, close socket\n");
        /* stop SOEM, close socket */
        ec_close();
    }
    else
    {
        printf("No socket connection on %s\nExecute as root\n",ifname);
    }
}

OSAL_THREAD_FUNC ecatcheck( void *ptr )
{
    int slave;
    (void)ptr;                  /* Not used */

    while(1)
    {
        if( inOP && ((wkc < expectedWKC) || ec_group[currentgroup].docheckstate))
        {
            if (needlf)
            {
               needlf = FALSE;
               printf("\n");
            }
            /* one ore more slaves are not responding */
            ec_group[currentgroup].docheckstate = FALSE;
            ec_readstate();
            for (slave = 1; slave <= ec_slavecount; slave++)
            {
               if ((ec_slave[slave].group == currentgroup) && (ec_slave[slave].state != EC_STATE_OPERATIONAL))
               {
                  ec_group[currentgroup].docheckstate = TRUE;
                  if (ec_slave[slave].state == (EC_STATE_SAFE_OP + EC_STATE_ERROR))
                  {
                     printf("ERROR : slave %d is in SAFE_OP + ERROR, attempting ack.\n", slave);
                     ec_slave[slave].state = (EC_STATE_SAFE_OP + EC_STATE_ACK);
                     ec_writestate(slave);
                  }
                  else if(ec_slave[slave].state == EC_STATE_SAFE_OP)
                  {
                     printf("WARNING : slave %d is in SAFE_OP, change to OPERATIONAL.\n", slave);
                     ec_slave[slave].state = EC_STATE_OPERATIONAL;
                     ec_writestate(slave);
                  }
                  else if(ec_slave[slave].state > EC_STATE_NONE)
                  {
                     if (ec_reconfig_slave(slave, EC_TIMEOUTMON))
                     {
                        ec_slave[slave].islost = FALSE;
                        printf("MESSAGE : slave %d reconfigured\n",slave);
                     }
                  }
                  else if(!ec_slave[slave].islost)
                  {
                     /* re-check state */
                     ec_statecheck(slave, EC_STATE_OPERATIONAL, EC_TIMEOUTRET);
                     if (ec_slave[slave].state == EC_STATE_NONE)
                     {
                        ec_slave[slave].islost = TRUE;
                        printf("ERROR : slave %d lost\n",slave);
                     }
                  }
               }
               if (ec_slave[slave].islost)
               {
                  if(ec_slave[slave].state == EC_STATE_NONE)
                  {
                     if (ec_recover_slave(slave, EC_TIMEOUTMON))
                     {
                        ec_slave[slave].islost = FALSE;
                        printf("MESSAGE : slave %d recovered\n",slave);
                     }
                  }
                  else
                  {
                     ec_slave[slave].islost = FALSE;
                     printf("MESSAGE : slave %d found\n",slave);
                  }
               }
            }
            if(!ec_group[currentgroup].docheckstate)
               printf("OK : all slaves resumed OPERATIONAL.\n");
        }
        osal_usleep(10000);
    }
}

int main(void)
{
   printf("SOEM (Simple Open EtherCAT Master)\n");  
    needlf = FALSE;
    inOP = FALSE;

   printf("Starting simple test\n");
   char *ifname = "eth1";
   /* initialise SOEM, bind socket to ifname */
   if (ec_init(ifname))
   {
      if ( ec_config(FALSE, &IOmap) > 0 )
      if ( ec_config_init(FALSE) > 0 )
         printf("ec_init on %s succeeded.\n",ifname);
      /* find and auto-config slaves */
       ec_readstate();
      printf("%d slaves found and configured.\n",ec_slavecount);

      ec_config_map(&IOmap);

      ec_configdc();      
      for(int i = 1;i <= ec_slavecount; i++)
      {
         ec_readstate();
         printf("state = \t%d\n",ec_slave[i].state);
         printf("ALstatuscode = \t%d\n",ec_slave[i].ALstatuscode);
         printf("configadr = \t%d\n",ec_slave[i].configadr);
         printf("aliasadr = \t%d\n",ec_slave[i].aliasadr);
         printf("eep_man = \t%d\n",ec_slave[i].eep_man);
         printf("eep_id = \t%d\n",ec_slave[i].eep_id);
         printf("eep_rev = \t%d\n",ec_slave[i].eep_rev);
         printf("Itype = \t%d\n",ec_slave[i].Itype);
         printf("Dtype = \t%d\n",ec_slave[i].Dtype);
         printf("Obits = \t%d\n",ec_slave[i].Obits);
         printf("Obytes = \t%d\n",ec_slave[i].Obytes);
         printf("outputs = \t%p\n",ec_slave[i].outputs);
         printf("Ostartbit = \t%d\n",ec_slave[i].Ostartbit);
         printf("Ibits = \t%d\n",ec_slave[i].Ibits);
         printf("Ibytes = \t%d\n",ec_slave[i].Ibytes);
         printf("inputs* = \t%p\n",ec_slave[i].inputs);
         printf("Istartbit = \t%d\n",ec_slave[i].Istartbit);
         printf("FMMU0func = \t%d\n",ec_slave[i].FMMU0func);
         printf("FMMU1func = \t%d\n",ec_slave[i].FMMU1func);
         printf("FMMU2func = \t%d\n",ec_slave[i].FMMU2func);
         printf("FMMU3func = \t%d\n",ec_slave[i].FMMU3func);
         printf("mbx_l = \t%d\n",ec_slave[i].mbx_l);
         printf("mbx_wo = \t%d\n",ec_slave[i].mbx_wo);
         printf("mbx_rl = \t%d\n",ec_slave[i].mbx_rl);
         printf("mbx_ro = \t%d\n",ec_slave[i].mbx_ro);
         printf("mbx_proto = \t%d\n",ec_slave[i].mbx_proto);
         printf("mbx_cnt = \t%d\n",ec_slave[i].mbx_cnt);
         printf("hasdc = \t%d\n",ec_slave[i].hasdc);
         printf("ptype = \t%d\n",ec_slave[i].ptype);
         printf("topology = \t%d\n",ec_slave[i].topology);
         printf("activeports = \t%d\n",ec_slave[i].activeports);
         printf("consumedports =\t%d\n",ec_slave[i].consumedports);
         printf("parent = \t%d\n",ec_slave[i].parent);
         printf("parentport = \t%d\n",ec_slave[i].parentport);
         printf("entryport = \t%d\n",ec_slave[i].entryport);
         printf("DCrtA = \t%d\n",ec_slave[i].DCrtA);
         printf("DCrtB = \t%d\n",ec_slave[i].DCrtB);
         printf("DCrtC = \t%d\n",ec_slave[i].DCrtC);
         printf("DCrtD = \t%d\n",ec_slave[i].DCrtD);
         printf("pdelay = \t%d\n",ec_slave[i].pdelay);
         printf("DCnext = \t%d\n",ec_slave[i].DCnext);
         printf("DCprevious = \t%d\n",ec_slave[i].DCprevious);
         printf("DCcycle = \t%d\n",ec_slave[i].DCcycle);
         printf("DCshift = \t%d\n",ec_slave[i].DCshift);
         printf("DCactive = \t%d\n",ec_slave[i].DCactive);
         printf("configindex = \t%d\n",ec_slave[i].configindex);
         printf("SIIindex = \t%d\n",ec_slave[i].SIIindex);
         printf("eep_8byte = \t%d\n",ec_slave[i].eep_8byte);
         printf("eep_pdi = \t%d\n",ec_slave[i].eep_pdi);
         printf("CoEdetails = \t%d\n",ec_slave[i].CoEdetails);
         printf("FoEdetails = \t%d\n",ec_slave[i].FoEdetails);
         printf("EoEdetails = \t%d\n",ec_slave[i].EoEdetails);
         printf("SoEdetails = \t%d\n",ec_slave[i].SoEdetails);
         printf("Ebuscurrent = \t%d\n",ec_slave[i].Ebuscurrent);
         printf("blockLRW = \t%d\n",ec_slave[i].blockLRW);
         printf("group = \t%d\n",ec_slave[i].group);
         printf("FMMUunused = \t%d\n",ec_slave[i].FMMUunused);
         printf("islost = \t%d\n",ec_slave[i].islost);
         printf("name = \t%s\n",ec_slave[i].name);
      }
      while(1)
      {
         
      }
   }
   return (0);
}
      // while(1)
      // {            
      //    ec_send_processdata();
      //    ec_receive_processdata(EC_TIMEOUTRET);
      //    osal_usleep(100000);        
      //    if(ec_slave[0].outputs[0] )
      //    {
      //       memset(ec_slave[0].outputs,0x00,8);
      //    }else
      //    {
      //       memset(ec_slave[0].outputs,0xff,8);
      //    }
      //    //ec_slave
      //    // printf("ec_slave = %d \n",ec_slave[0].outputs[0]);
      //    printf("ec_slave[0].outputs = %p\n",ec_slave[0].outputs);         
      //    printf("IOmap               = %p\n",IOmap);
      // }



//    if (argc > 1)
//    {
//       /* create thread to handle slave error handling in OP */
// //      pthread_create( &thread1, NULL, (void *) &ecatcheck, (void*) &ctime);
//       osal_thread_create(&thread1, 128000, &ecatcheck, (void*) &ctime);
//       /* start cyclic part */
//       simpletest(argv[1]);
//    }
//    else
//    {
//       printf("Usage: simple_test ifname1\nifname = eth0 for example\n");
//    }

//    printf("End program\n");